//---------------------------------------------------------------------------


#pragma hdrstop

#include "stdafx.h"
#include "Motion.h"

//---------------------------------------------------------------------------

#pragma package(smart_init)

MotionStruct Motion = {0};

/*******************************************************************************
* Function Name  : Motion_Init
* Description    : Initialize Motion
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/
void Motion_Init(void)
{
	Motion.Mode = NO_MOTION;
	Motion.Mode_RX = NO_MOTION;
	Motion.Mode_Past = NO_MOTION;
	Motion.Doing = NO_MOTION;
	Motion.Replaceable = TRUE;

	Motion.Continue_Eanble = FALSE;
	Motion.Continue_Set = FALSE;

	Motion.ContinueAngle = 0;
	Motion.ContinueDistance = 0;
	Motion.ContinueAngle_Past = 0;
	Motion.ContinueDistance_Past = 0;
	Motion.ContinueAngle_RX = 0;
	Motion.ContinueDistance_RX = 0;

	Motion.DoneX = 0;
	Motion.DoneY = 0;
	Motion.DoneAngle = 0;

	for(int i=0; i < MOTION_QUEUE_SIZE; i++)
	{
    Motion.Queue[i] = 0;
  }
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/*******************************************************************************
* Function Name  : SingleMotion
* Description    : Add the content of single motion into the MotorSchedule
* Input          : None
* Output         : None
* Return         : None
* Attention		 : 	For fallen & get up
									  motion_doing = 255;
									For single motion
									  motion_doing = motion_mode;
									For continuous motion
									  motion_doing = cont_set;  // 0 for motion done
*******************************************************************************/
void SingleMotion(void)
{
	switch(Motion.Mode)
	{
		#ifdef SIDEKICK_R
      case SIDEKICK_R:
        Mtn_SideKick_R();
        break;
    #endif

    #ifdef SIDEKICK_L
      case SIDEKICK_L:
        Mtn_SideKick_L();
        break;
    #endif

    #ifdef WALK_F
      case WALK_F:
        Mtn_Walk_F();
        break;
    #endif

    #ifdef WALK_B
      case WALK_B:
        Mtn_Walk_B();
        break;
    #endif

    #ifdef WALK_FF
      case WALK_FF:
        Mtn_Walk_FF();
        break;
    #endif

    #ifdef WALK_BB
      case WALK_BB:
        Mtn_Walk_BB();
        break;
    #endif

    #ifdef WALK_FFF
      case WALK_FFF:
        Mtn_Walk_FFF();
        break;
    #endif

    #ifdef WALK_BBB
      case WALK_BBB:
        Mtn_Walk_BBB();
        break;
    #endif

    #ifdef SHIFT_R
      case SHIFT_R:
        Mtn_Shift_R();
        break;
    #endif

    #ifdef SHIFT_L
      case SHIFT_L:
        Mtn_Shift_L();
        break;
    #endif

    #ifdef SHIFT_RR
      case SHIFT_RR:
        Mtn_Shift_RR();
        break;
    #endif

    #ifdef SHIFT_LL
      case SHIFT_LL:
        Mtn_Shift_LL();
        break;
    #endif

    #ifdef SHIFT_RRR
      case SHIFT_RRR:
        Mtn_Shift_RRR();
        break;
    #endif

    #ifdef SHIFT_LLL
      case SHIFT_LLL:
        Mtn_Shift_LLL;
        break;
    #endif

    #ifdef TURN_R
      case TURN_R:
        Mtn_Turn_R();
        break;
    #endif

    #ifdef TURN_L
      case TURN_L:
        Mtn_Turn_L();
        break;
    #endif

    #ifdef TURN_RR
      case TURN_RR:
        Mtn_Turn_RR();
        break;
    #endif

    #ifdef TURN_LL
      case TURN_LL:
        Mtn_Turn_LL();
        break;
    #endif

    #ifdef TURN_RRR
      case TURN_RRR:
        Mtn_Turn_RRR();
        break;
    #endif

    #ifdef TURN_LLL
      case TURN_LLL:
        Mtn_Turn_LLL;
        break;
    #endif

    #ifdef TURNBALL_R
      case TURNBALL_R:
        Mtn_TurnBall_R();
        break;
    #endif

    #ifdef TURNBALL_L
      case TURNBALL_L:
        Mtn_TurnBall_L();
        break;
    #endif

    #ifdef KICK_R
      case KICK_R:
        Mtn_Kick_R();
        break;
    #endif

    #ifdef KICK_L
      case KICK_L:
        Mtn_Kick_L();
        break;
    #endif

    #ifdef KICK_RR
      case KICK_RR:
        Mtn_Kick_RR();
        break;
    #endif

    #ifdef KICK_LL
      case KICK_LL:
        Mtn_Kick_LL();
        break;
    #endif

    #ifdef KICK_RRR
      case KICK_RRR:
        Mtn_Kick_RRR();
        break;
    #endif

    #ifdef KICK_LLL
      case KICK_LLL:
        Mtn_Kick_LLL();
        break;
    #endif

		#ifdef LIFT_L
			case LIFT_L:
				Mtn_Lift_L();
				break;
		#endif

		#ifdef LIFT_H
			case LIFT_H:
				Mtn_Lift_H();
				break;
		#endif

		#ifdef LIFT_L_WALK_F
			case LIFT_L_WALK_F:
				Mtn_Lift_L_Walk_F();
				break;
		#endif

		#ifdef LIFT_H_WALK_F
			case LIFT_H_WALK_F:
				Mtn_Lift_H_Walk_F();
				break;
		#endif

		#ifdef LIFT_L_TRUN_R
			case LIFT_L_TRUN_R:
				Mtn_Lift_L_Turn_R();
				break;
		#endif

		#ifdef LIFT_L_TRUN_L
			case LIFT_L_TRUN_L:
				Mtn_Lift_L_Turn_L();
				break;
		#endif

    #ifdef GETUP_F
      case GETUP_F:
        Mtn_GetUp_F();
        break;
    #endif

    #ifdef GETUP_B
      case GETUP_B:
        Mtn_GetUp_B();
        break;
    #endif

    #ifdef UP_STAIR
      case UP_STAIR:
        Mtn_Up_Stair();
        break;
    #endif

    #ifdef DOWN_STAIR
      case DOWN_STAIR:
        Mtn_Down_Stair();
        break;
    #endif

    #ifdef SQUAT
      case SQUAT:
        Mtn_Squat();
        break;
    #endif

    #ifdef STANDUP
      case STANDUP:
        Mtn_StandUp();
        break;
    #endif

    #ifdef PICKUP
      case PICKUP:
        Mtn_PickUp();
        break;
    #endif

    #ifdef PUT
      case PUT:
        Mtn_Put();
        break;
    #endif

    #ifdef BASKETBALL
			#ifdef THROW
      case THROW:
        Mtn_Throw(Motion.ContinueDistance);
        Mtn_POSE_Standing(50, 0, 17);
        break;
			#endif
    #else
			#ifdef THROW
      case THROW:
        Mtn_ThrowBall();
        break;
			#endif
    #endif

    #ifdef ROTATE_WAIST
      case ROTATE_WAIST:
        Mtn_RotateWaist(Motion.ContinueAngle);
        break;
    #endif

    #ifdef UNEVEN
    case UNEVEN:
      Mtn_Uneven();
      break;
    #endif

    #ifdef LIFT
    case LIFT:
      Event_Lift();
      break;
    #endif

    #ifdef CLIMBWALL
    case CLIMBWALL:
      Event_ClimbWall();
      break;
    #endif

    #ifdef RAISE_ARM
    case RAISE_ARM:
      Mtn_Raise_Arm();
      break;
    #endif

    #ifdef GETDOWN_ARM
    case GETDOWN_ARM:
      Mtn_Getdown_Arm();
			Mtn_POSE_Standing(50, 0, 17);
      break;
    #endif

		#ifdef STAND_PICKUP
    case STAND_PICKUP:
      Mtn_STAND_PICKUP();
      break;
		#endif

    case POS_STAND:
      Mtn_POSE_Standing(100, 0, 5);
      break;
    case POS_STAND_NOHEAD:
      Mtn_POSE_Standing_Nohead(100, 0, 5);
      break;
    default:
      Motion.Mode = NO_MOTION;
			Motion.Doing = NO_MOTION;
      break;
	}

	Motion.Doing = Motion.Mode;
	Motion.Mode_Past = Motion.Mode;
	Motion.Mode = NO_MOTION;
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/*******************************************************************************
* Function Name  : ContinuousMotion
* Description    : Add the content of continuous motion into the MotorSchedule
* Input          : None
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void ContinuousMotion(void)
{
	if(MotorSchedule.NowIndex == MotorSchedule.EndIndex)
	{
    if(Motion.Mode > STOP)
		{
			if(Motion.Continue_Set == OFF)
			{
				Motion.Mode = NO_MOTION;
				Motion.ContinueDistance = 0;
        Motion.ContinueAngle = 0;
			}
      Motion.Doing = Motion.Continue_Set;
    }

		if(Motion.Continue_Set != OFF)
		{
			if(Motion.Mode_Past != Motion.Mode)
			{
				// Check the change of speed
				if(Motion.Mode == WALK)
				{ // The other -> WALK : Speed up
					if(Motion.ContinueDistance > 0)
					{
						#ifdef C_SPEEDUP_F
							MtnC_SpeedUp_F();
						#endif
					}
					else if(Motion.ContinueDistance < 0)
					{
						#ifdef C_SPEEDUP_B
							MtnC_SpeedUp_B();
						#endif
					}
				}
				else if(Motion.Mode_Past == WALK)
				{  // WALK -> The other : Speed down
					if(Motion.ContinueDistance_Past > 0)
					{
						#ifdef C_SPEEDDOWN_F
							MtnC_SpeedDown_F();
						#endif
					}
					else if(Motion.ContinueDistance_Past < 0)
					{
						#ifdef C_SPEEDDOWN_B
							MtnC_SpeedDown_B();
						#endif
					}
				}

				if(Motion.Mode == WALK_RAISEARM)
				{
					if(Motion.ContinueDistance > 0)
					{
						#ifdef C_SPEEDUP_F_RAISEARM
							MtnC_SpeedUp_F_RaiseArm();
						#endif
					}
					else if(Motion.ContinueDistance < 0)
					{
						#ifdef C_SPEEDUP_B_RAISEARM
							MtnC_SpeedUp_B_RaiseArm();
						#endif
					}
				}
				else if(Motion.Mode_Past == WALK_RAISEARM)
				{
					if(Motion.ContinueDistance_Past > 0)
					{
						#ifdef C_SPEEDDOWN_F_RAISEARM
							MtnC_SpeedDown_F_RaiseArm();
						#endif
					}
					else if(Motion.ContinueDistance_Past < 0)
					{
						#ifdef C_SPEEDDOWN_B_RAISEARM
							MtnC_SpeedDown_B_RaiseArm();
						#endif
					}
				}
			}
			else if(Motion.Mode == WALK)
			{
				// WALK -> WALK : Check the change of direction
				if( (Motion.ContinueDistance_Past > 0) && (Motion.ContinueDistance < 0) )
				{
					// F -> B
					// Reserve
				}
				else if( (Motion.ContinueDistance_Past < 0) && (Motion.ContinueDistance > 0) )
				{
					// B -> F
					// Reserve
				}
				else if( ((Motion.ContinueAngle_Past > 0) && (Motion.ContinueAngle < 0)) ||
								 ((Motion.ContinueAngle_Past < 0) && (Motion.ContinueAngle > 0)) )
				{
					// R <-> L, prevent the motion becoming unstable
					if(Motion.ContinueDistance >= 0)
					{
						#ifdef C_WALK_F
							MtnC_Walk_F();
						#endif
					}
					else
					{
						#ifdef C_WALK_B
							MtnC_Walk_B();
						#endif
					}
				}
			}
			else if(Motion.Mode == WALK_RAISEARM)
			{
				// WALK -> WALK : Check the change of direction
				if( (Motion.ContinueDistance_Past > 0) && (Motion.ContinueDistance < 0) )
				{
					// F -> B
					// Reserve
				}
				else if( (Motion.ContinueDistance_Past < 0) && (Motion.ContinueDistance > 0) )
				{
					// B -> F
					// Reserve
				}
				else if(((Motion.ContinueAngle_Past > 0) && (Motion.ContinueAngle < 0)) ||
								((Motion.ContinueAngle_Past < 0) && (Motion.ContinueAngle > 0)) )
				{
					// R <-> L
					if(Motion.ContinueDistance >= 0)
					{
						#ifdef C_WALK_F_RAISEARM
							MtnC_Walk_F_RaiseArm();
						#endif
					}
					else
					{
						#ifdef C_WALK_B_RAISEARM
							MtnC_Walk_B_RaiseArm();
						#endif
					}
				}
			}
		}
		Motion.ContinueDistance_Past = Motion.ContinueDistance;
		Motion.ContinueAngle_Past = Motion.ContinueAngle;

		switch(Motion.Mode)
		{
			case STEP:
				#ifdef C_STEP_H
					MtnC_Step();
        #endif
				Motion.Continue_Set = OFF;
				break;


			case WALK:
				if(Motion.ContinueDistance >= WALK_F_DISTANCE)
				{
					if( (Motion.ContinueAngle > WALK_FL_ANGLE) &&
							(Motion.ContinueAngle < WALK_FR_ANGLE) )
					{
						#ifdef C_WALK_F
							MtnC_Walk_F();
						#endif
						Motion.ContinueDistance -= WALK_F_DISTANCE;
						if(Motion.ContinueDistance < WALK_F_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
					else if(Motion.ContinueAngle >= WALK_FR_ANGLE)
					{
						#ifdef C_WALK_FR
							MtnC_Walk_FR();
						#endif
						Motion.ContinueDistance -= WALK_FR_DISTANCE;
						Motion.ContinueAngle -= WALK_FR_ANGLE;
						if(Motion.ContinueDistance < WALK_FR_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
					else if(Motion.ContinueAngle <= WALK_FL_ANGLE)
					{
						#ifdef C_WALK_FL
							MtnC_Walk_FL();
						#endif
						Motion.ContinueDistance -= WALK_FL_DISTANCE;
						Motion.ContinueAngle -= WALK_FL_ANGLE;
						if(Motion.ContinueDistance < WALK_FL_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
				}
				else if(Motion.ContinueDistance <= WALK_B_DISTANCE)
				{
					if( (Motion.ContinueAngle > WALK_BL_ANGLE) &&
							(Motion.ContinueAngle < WALK_BR_ANGLE))
					{
						#ifdef C_WALK_B
							MtnC_Walk_B();
						#endif
						Motion.ContinueDistance -= WALK_B_DISTANCE;
						if(Motion.ContinueDistance > WALK_B_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
					else if(Motion.ContinueAngle >= WALK_BR_ANGLE)
					{
						#ifdef C_WALK_BR
							MtnC_Walk_BR();
						#endif
						Motion.ContinueDistance -= WALK_BR_DISTANCE;
						Motion.ContinueAngle -= WALK_BR_ANGLE;
						if(Motion.ContinueDistance > WALK_BR_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
					else if(Motion.ContinueAngle <= WALK_BL_ANGLE)
					{
						#ifdef C_WALK_BL
							MtnC_Walk_BL();
						#endif
						Motion.ContinueDistance -= WALK_BL_DISTANCE;
						Motion.ContinueAngle -= WALK_BL_ANGLE;
						if(Motion.ContinueDistance > WALK_BL_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
				}
				else
				{
					Motion.Continue_Set = OFF;
				}
				break;


			case WALK_RAISEARM:
				if(Motion.ContinueDistance >= WALK_F_DISTANCE)
				{
					if( (Motion.ContinueAngle > WALK_FL_ANGLE) &&
							(Motion.ContinueAngle < WALK_FR_ANGLE))
					{
						#ifdef C_WALK_F_RAISEARM
							MtnC_Walk_F_RaiseArm();
						#endif
						Motion.ContinueDistance -= WALK_F_DISTANCE;
						if(Motion.ContinueDistance < WALK_F_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
					else if(Motion.ContinueAngle >= WALK_FR_ANGLE)
					{
						#ifdef C_WALK_FR_RAISEARM
							MtnC_Walk_FR_RaiseArm();
						#endif
						Motion.ContinueDistance -= WALK_FR_DISTANCE;
						Motion.ContinueAngle -= WALK_FR_ANGLE;
						if(Motion.ContinueDistance < WALK_FR_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
					else if(Motion.ContinueAngle <= WALK_FL_ANGLE)
					{
						#ifdef C_WALK_FL_RAISEARM
							MtnC_Walk_FL_RaiseArm();
						#endif
						Motion.ContinueDistance -= WALK_FL_DISTANCE;
						Motion.ContinueAngle -= WALK_FL_ANGLE;
						if(Motion.ContinueDistance < WALK_FL_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
				}
				else if(Motion.ContinueDistance <= WALK_B_DISTANCE)
				{
					if( (Motion.ContinueAngle > WALK_BL_ANGLE) &&
							(Motion.ContinueAngle < WALK_BR_ANGLE))
					{
						#ifdef C_WALK_B_RAISEARM
							MtnC_Walk_B_RaiseArm();
						#endif
						Motion.ContinueDistance -= WALK_B_DISTANCE;
						if(Motion.ContinueDistance > WALK_B_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
					else if(Motion.ContinueAngle >= WALK_BR_ANGLE)
					{
						#ifdef C_WALK_BR_RAISEARM
							MtnC_Walk_BR_RaiseArm();
						#endif
						Motion.ContinueDistance -= WALK_BR_DISTANCE;
						Motion.ContinueAngle -= WALK_BR_ANGLE;
						if(Motion.ContinueDistance > WALK_BR_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
					else if(Motion.ContinueAngle <= WALK_BL_ANGLE)
					{
						#ifdef C_WALK_BL_RAISEARM
							MtnC_Walk_BL_RaiseArm();
						#endif
						Motion.ContinueDistance -= WALK_BL_DISTANCE;
						Motion.ContinueAngle -= WALK_BL_ANGLE;
						if(Motion.ContinueDistance > WALK_BL_DISTANCE)
						{
							Motion.Continue_Set = OFF;
						}
					}
				}
				else
				{
					Motion.Continue_Set = OFF;
				}
				break;


			case STOP:
				Motion.Continue_Eanble = FALSE;
				Motion.Continue_Set = OFF;
				break;


			default:
				Motion.Continue_Eanble = FALSE;
				Motion.Continue_Set = OFF;
				Motion.Mode = NO_MOTION;
				break;

		} // End of switch Motion.Mode
	} // End of if

	//Motion.Doing = Motion.Mode;
	Motion.Mode_Past = Motion.Mode;
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/*******************************************************************************
* Function Name  : Mtn_POSE_Standing
* Description    : Initial pose
* Input          : Speed and step time
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void Mtn_POSE_Standing(int speedL, int speedH, int steptime)
{
#if defined TEEN
	Add_motor_job(253,steptime,0,0,0);
  Add_motor_job(0,0,2,speedL,speedH);
  Add_motor_job(1,255,1,speedL,speedH);
  Add_motor_job(2,255,7,speedL,speedH);
  Add_motor_job(3,255,1,speedL,speedH);
  Add_motor_job(10,255,7,speedL,speedH);
  Add_motor_job(11,255,1,speedL,speedH);
  Add_motor_job(12,255,1,speedL,speedH);
  Add_motor_job(13,255,7,speedL,speedH);
  Add_motor_job(14,255,1,speedL,speedH);
  Add_motor_job(20,255,7,speedL,speedH);
  Add_motor_job(21,255,1,speedL,speedH);
  Add_motor_job(22,255,1,speedL,speedH);
  Add_motor_job(23,255,7,speedL,speedH);
	Add_motor_job(24,255,1,speedL,speedH);
	Add_motor_job(30,255,7,speedL,speedH);
	Add_motor_job(31,255,7,speedL,speedH);
	Add_motor_job(32,0,6,speedL,speedH);
	Add_motor_job(33,0,2,speedL,speedH);
	Add_motor_job(34,0,10,speedL,speedH);
	Add_motor_job(35,255,7,speedL,speedH);
	Add_motor_job(40,255,7,speedL,speedH);
	Add_motor_job(41,255,7,speedL,speedH);
	Add_motor_job(42,0,10,speedL,speedH);
	Add_motor_job(43,0,14,speedL,speedH);
	Add_motor_job(44,0,6,speedL,speedH);
	Add_motor_job(45,255,7,speedL,speedH);
	Add_motor_job(255,5,0,speedL,speedH);
#elif defined KID
	Add_motor_job(253,steptime,0,0,0);
	Add_motor_job(0,0,2,speedL,speedH);
	Add_motor_job(1,0,2,speedL,speedH);
	Add_motor_job(25,0,2,speedL,speedH);
	Add_motor_job(3,0,2,speedL,speedH);
	Add_motor_job(4,0,2,speedL,speedH);
	Add_motor_job(5,0,2,speedL,speedH);
	Add_motor_job(6,0,2,speedL,speedH);
	Add_motor_job(7,0,2,speedL,speedH);
	Add_motor_job(8,0,2,speedL,speedH);
	Add_motor_job(9,0,2,speedL,speedH);
	Add_motor_job(10,0,2,speedL,speedH);
	Add_motor_job(11,0,2,speedL,speedH);
	Add_motor_job(12,0,2,speedL,speedH);
	Add_motor_job(13,0,2,speedL,speedH);
	Add_motor_job(14,0,8,speedL,speedH);
	Add_motor_job(15,0,2,speedL,speedH);
	Add_motor_job(16,0,2,speedL,speedH);
	Add_motor_job(17,0,2,speedL,speedH);
	Add_motor_job(18,0,2,speedL,speedH);
	Add_motor_job(19,0,2,speedL,speedH);
	Add_motor_job(20,0,8,speedL,speedH);
	Add_motor_job(21,0,2,speedL,speedH);
	Add_motor_job(22,0,2,speedL,speedH);
	Add_motor_job(23,0,2,speedL,speedH);
	Add_motor_job(24,0,2,speedL,speedH);
	Add_motor_job(255,5,0,speedL,speedH);
#endif
}
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
/*******************************************************************************
* Function Name  : Mtn_POSE_Standing
* Description    : Initial pose without moving the head
* Input          : Speed and step time
* Output         : None
* Return         : None
* Attention		 : None
*******************************************************************************/
void Mtn_POSE_Standing_Nohead(int speedL, int speedH, int steptime)
{
#if defined TEEN
	Add_motor_job(253,steptime,0,0,0);
  Add_motor_job(2,255,7,speedL,speedH);
  Add_motor_job(3,255,1,speedL,speedH);
  Add_motor_job(10,255,7,speedL,speedH);
  Add_motor_job(11,255,1,speedL,speedH);
  Add_motor_job(12,255,1,speedL,speedH);
  Add_motor_job(13,255,7,speedL,speedH);
  Add_motor_job(14,255,1,speedL,speedH);
  Add_motor_job(20,255,7,speedL,speedH);
  Add_motor_job(21,255,1,speedL,speedH);
  Add_motor_job(22,255,1,speedL,speedH);
  Add_motor_job(23,255,7,speedL,speedH);
	Add_motor_job(24,255,1,speedL,speedH);
	Add_motor_job(30,255,7,speedL,speedH);
	Add_motor_job(31,255,7,speedL,speedH);
	Add_motor_job(32,0,6,speedL,speedH);
	Add_motor_job(33,0,2,speedL,speedH);
	Add_motor_job(34,0,10,speedL,speedH);
	Add_motor_job(35,255,7,speedL,speedH);
	Add_motor_job(40,255,7,speedL,speedH);
	Add_motor_job(41,255,7,speedL,speedH);
	Add_motor_job(42,0,10,speedL,speedH);
	Add_motor_job(43,0,14,speedL,speedH);
	Add_motor_job(44,0,6,speedL,speedH);
	Add_motor_job(45,255,7,speedL,speedH);
	Add_motor_job(255,5,0,speedL,speedH);
#elif defined KID
	Add_motor_job(253,steptime,0,0,0);
	Add_motor_job(25,0,2,speedL,speedH);
	Add_motor_job(3,0,2,speedL,speedH);
	Add_motor_job(4,0,2,speedL,speedH);
	Add_motor_job(5,0,2,speedL,speedH);
	Add_motor_job(6,0,2,speedL,speedH);
	Add_motor_job(7,0,2,speedL,speedH);
	Add_motor_job(8,0,2,speedL,speedH);
	Add_motor_job(9,0,2,speedL,speedH);
	Add_motor_job(10,0,2,speedL,speedH);
	Add_motor_job(11,0,2,speedL,speedH);
	Add_motor_job(12,0,2,speedL,speedH);
	Add_motor_job(13,0,2,speedL,speedH);
	Add_motor_job(14,0,8,speedL,speedH);
	Add_motor_job(15,0,2,speedL,speedH);
	Add_motor_job(16,0,2,speedL,speedH);
	Add_motor_job(17,0,2,speedL,speedH);
	Add_motor_job(18,0,2,speedL,speedH);
	Add_motor_job(19,0,2,speedL,speedH);
	Add_motor_job(20,0,8,speedL,speedH);
	Add_motor_job(21,0,2,speedL,speedH);
	Add_motor_job(22,0,2,speedL,speedH);
	Add_motor_job(23,0,2,speedL,speedH);
	Add_motor_job(24,0,2,speedL,speedH);
	Add_motor_job(255,5,0,speedL,speedH);
#endif
}

AnsiString MotionMode2MotionName(unsigned char motion_mode)
{
	AnsiString motion_name = "Undefined";

	switch(motion_mode)
	{
		#ifdef SIDEKICK_R
			case SIDEKICK_R:
				motion_name = "SIDEKICK_R";
        break;
    #endif

		#ifdef SIDEKICK_L
      case SIDEKICK_L:
				motion_name = "SIDEKICK_L";
        break;
		#endif

		#ifdef WALK_F
			case WALK_F:
				motion_name = "WALK_F";
        break;
		#endif
		#ifdef WALK_B
			case WALK_B:
				motion_name = "WALK_B";
        break;
    #endif

		#ifdef WALK_FF
			case WALK_FF:
				motion_name = "WALK_FF";
        break;
    #endif

		#ifdef WALK_BB
			case WALK_BB:
				motion_name = "WALK_BB";
        break;
    #endif

		#ifdef WALK_FFF
			case WALK_FFF:
				motion_name = "WALK_FFF";
        break;
    #endif

		#ifdef WALK_BBB
			case WALK_BBB:
				motion_name = "WALK_BBB";
        break;
    #endif

		#ifdef SHIFT_R
			case SHIFT_R:
				motion_name = "SHIFT_R";
        break;
    #endif

		#ifdef SHIFT_L
			case SHIFT_L:
				motion_name = "SHIFT_L";
        break;
    #endif

		#ifdef SHIFT_RR
			case SHIFT_RR:
				motion_name = "SHIFT_RR";
        break;
    #endif

		#ifdef SHIFT_LL
			case SHIFT_LL:
				motion_name = "SHIFT_LL";
        break;
    #endif

		#ifdef SHIFT_RRR
			case SHIFT_RRR:
				motion_name = "SHIFT_RRR";
        break;
    #endif

		#ifdef SHIFT_LLL
			case SHIFT_LLL:
				motion_name = "SHIFT_LLL";
        break;
    #endif

		#ifdef TURN_R
			case TURN_R:
				motion_name = "TURN_R";
        break;
    #endif

		#ifdef TURN_L
			case TURN_L:
				motion_name = "TURN_L";
        break;
    #endif

		#ifdef TURN_RR
			case TURN_RR:
				motion_name = "TURN_RR";
        break;
    #endif

		#ifdef TURN_LL
			case TURN_LL:
				motion_name = "TURN_LL";
        break;
    #endif

		#ifdef TURN_RRR
			case TURN_RRR:
				motion_name = "TURN_RRR";
        break;
    #endif

		#ifdef TURN_LLL
			case TURN_LLL:
				motion_name = "TURN_LLL";
        break;
    #endif

		#ifdef TURNBALL_R
			case TURNBALL_R:
				motion_name = "TURNBALL_R";
        break;
    #endif

		#ifdef TURNBALL_L
			case TURNBALL_L:
				motion_name = "TURNBALL_L";
        break;
    #endif

		#ifdef KICK_R
			case KICK_R:
				motion_name = "KICK_R";
        break;
    #endif

		#ifdef KICK_L
			case KICK_L:
				motion_name = "KICK_L";
        break;
    #endif

		#ifdef KICK_RR
			case KICK_RR:
				motion_name = "KICK_RR";
        break;
    #endif

		#ifdef KICK_LL
			case KICK_LL:
				motion_name = "KICK_LL";
        break;
    #endif

		#ifdef KICK_RRR
			case KICK_RRR:
				motion_name = "KICK_RRR";
        break;
    #endif

		#ifdef KICK_LLL
			case KICK_LLL:
				motion_name = "KICK_LLL";
        break;
    #endif

		#ifdef LIFT_L
			case LIFT_L:
				motion_name = "LIFT_L";
				break;
		#endif

		#ifdef LIFT_H
			case LIFT_H:
				motion_name = "LIFT_H";
				break;
		#endif

		#ifdef LIFT_L_WALK_F
			case LIFT_L_WALK_F:
				motion_name = "LIFT_L_WALK_F";
				break;
		#endif

		#ifdef LIFT_H_WALK_F
			case LIFT_H_WALK_F:
				motion_name = "LIFT_H_WALK_F";
				break;
		#endif

		#ifdef LIFT_L_TRUN_R
			case LIFT_L_TRUN_R:
				motion_name = "LIFT_L_TRUN_R";
				break;
		#endif

		#ifdef LIFT_L_TRUN_L
			case LIFT_L_TRUN_L:
				motion_name = "LIFT_L_TRUN_L";
				break;
		#endif

		#ifdef GETUP_F
			case GETUP_F:
				motion_name = "GETUP_F";
        break;
    #endif

		#ifdef GETUP_B
			case GETUP_B:
				motion_name = "GETUP_B";
        break;
    #endif

		#ifdef UP_STAIR
      case UP_STAIR:
				motion_name = "UP_STAIR";
        break;
    #endif

		#ifdef DOWN_STAIR
			case DOWN_STAIR:
				motion_name = "DOWN_STAIR";
        break;
    #endif

		#ifdef SQUAT
			case SQUAT:
				motion_name = "SQUAT";
        break;
    #endif

		#ifdef STANDUP
			case STANDUP:
				motion_name = "STANDUP";
        break;
    #endif

		#ifdef PICKUP
			case PICKUP:
				motion_name = "PICKUP";
        break;
    #endif

    #ifdef PUT
			case PUT:
				motion_name = "PUT";
        break;
    #endif

		#ifdef BASKETBALL
			#ifdef THROW
      case THROW:
				motion_name = "BASKETBALL THROW";
        break;
			#endif
    #else
			#ifdef THROW
			case THROW:
				motion_name = "THROW";
        break;
			#endif
    #endif

    #ifdef ROTATE_WAIST
			case ROTATE_WAIST:
				motion_name = "ROTATE_WAIST";
        break;
    #endif

		#ifdef UNEVEN
		case UNEVEN:
				motion_name = "UNEVEN";
      break;
    #endif

		#ifdef LIFT
		case LIFT:
				motion_name = "LIFT";
      break;
    #endif

		#ifdef CLIMBWALL
		case CLIMBWALL:
				motion_name = "CLIMBWALL";
      break;
    #endif

		#ifdef RAISE_ARM
		case RAISE_ARM:
				motion_name = "RAISE_ARM";
      break;
    #endif

		#ifdef GETDOWN_ARM
		case GETDOWN_ARM:
				motion_name = "GETDOWN_ARM";
      break;
    #endif

		#ifdef STAND_PICKUP
		case STAND_PICKUP:
				motion_name = "STAND_PICKUP";
			break;
		#endif

		#ifdef POS_STAND
		case POS_STAND:
				motion_name = "POS_STAND";
			break;
		#endif

		#ifdef POS_STAND_NOHEAD
		case POS_STAND_NOHEAD:
				motion_name = "POS_STAND_NOHEAD";
			break;
		#endif

		#ifdef STOP
		case STOP:
				motion_name = "STOP";
			break;
		#endif

		#ifdef STEP
		case STEP:
				motion_name = "STEP";
			break;
		#endif

		#ifdef SHIFT
		case SHIFT:
				motion_name = "SHIFT";
			break;
		#endif

		#ifdef TURN
		case TURN:
				motion_name = "TURN";
			break;
		#endif

		#ifdef TURNBALL
		case TURNBALL:
				motion_name = "TURNBALL";
			break;
		#endif

		#ifdef WALK
		case WALK:
				motion_name = "WALK";
			break;
		#endif

		#ifdef WALK_RAISEARM
		case WALK_RAISEARM:
				motion_name = "WALK_RAISEARM";
			break;
		#endif

		#ifdef TORQUEOFF
		case TORQUEOFF:
				motion_name = "TORQUEOFF";
			break;
		#endif

		#ifdef GETINGUP
		case GETINGUP:
				motion_name = "GETINGUP";
			break;
		#endif

		default:
				motion_name = "Undefined";
			break;
	}

	return motion_name;
}

